#include "Rope.h"
#include "include.h"

Rope::Rope(Vector2D * anchor, StringSegment * segments, int numSegments)
: String(anchor, segments, numSegments)
{
	pastMinLength = false;
	controllingObject = false;
	grabbingObject = false;
	pauseUntilShrink = false;
	grabPoint = NULL;
	grabObject = NULL;
	checkCollision = false;
}

Rope::Rope(Vector2D * anchor, int numSegments, float mass, float length, float springConstant)
: String(anchor, numSegments, mass, length, springConstant)
{
	pastMinLength = false;
	controllingObject = false;
	grabbingObject = false;
	pauseUntilShrink = false;
	grabPoint = NULL;
	grabObject = NULL;
	checkCollision = false;
}

Rope::~Rope()
{
	/*for(int i = 0; i < numSegments; i++)
	{
		delete(&segments[i]);
	}
	delete(segments);
	segments = NULL;
	numSegments = 0;
	delete(anchor);
	anchor = NULL;*/
}
void Rope::Grab(Object * object, Vector2D * point)
{
	grabObject = object;
	grabObject->PickingUpObject();
	grabPoint = point;
	//printf("add grab point = (%f, %f)\n", grabPoint->x, grabPoint->y);
	//for(int i = 0; i < numSegments; i++)
	//	segments[i].SetDampingConstant(object->mass);
	pauseUntilShrink = false;
	grabbingObject = true;
	checkCollision = false;
}
void Rope::Release()
{
	if(grabbingObject)
	{
		grabbingObject = false;
		grabObject->ReleasingObject();
		grabObject->RemoveKeyPoint(grabPoint);
		grabObject = NULL;
		grabPoint = NULL;
		segments[numSegments-1].ResetDampingConstant();
		checkCollision = false;
		pauseUntilShrink = true;
	}
}
Vector2D Rope::Think(double elapsedTime)
{
	Vector2D anchorForce = Vector2D(0,0);
	Vector2D lastPoint = *anchor;
	
	if(grabObject && grabObject->LetMeGo())
	{
		Release();
	}
	
	for(int i = 0; i < numSegments; i++)
	{
		Vector2D force;
		//printf("string force %d = (%f, %f)\n", i, force.x, force.y);
		if(grabbingObject && i == numSegments - 1)
		{
			
			if(i > 0)
				force = segments[i].CalculateStringForce(&lastPoint, segments[i-1].velocity, grabObject, grabPoint);
			else
				force = segments[i].CalculateStringForce(&lastPoint, grabObject, grabPoint);
			
				
			if(controllingObject && grabObject->objectId != PINNEDOBJECT_CLASS)
			{
				//	grabObject->AddForce(force);
			}
			else
				grabObject->AddForce(force, segments[i].endPoint);
			if(force.Magnitude() > BREAK_ROPE_FORCE/elapsedTime)
			{
				Release();
			}
		}
		else
		{
			if(i > 0)
				force = segments[i].CalculateStringForce(&lastPoint, segments[i-1].velocity);
			else
				force = segments[i].CalculateStringForce(&lastPoint);
			segments[i].AddForce(force);
		}
		if(i > 0)
		{
			segments[i-1].AddForce( -1.0 * force);
			if(segments[i - 1].endPoint.y > segments[i].endPoint.y)
			{
					if(grabbingObject && i == numSegments - 1)
					{
						force = segments[i].CalculateGravitationalForce(&lastPoint, grabObject);
					}
					else
						force = segments[i].CalculateGravitationalForce(&lastPoint);
					segments[i - 1].AddForce(force);
			}
		}
		else
		{
			anchorForce = anchorForce + ( -1.0 * force);
			if(anchor->y  > segments[i].endPoint.y)
			{
				force = segments[i].CalculateGravitationalForce(&lastPoint);
				anchorForce = anchorForce + force;
			}
		}
		
		
		if(grabbingObject && i == numSegments-1)
			lastPoint = *grabPoint + grabObject->position;
		else
			lastPoint = segments[i].endPoint;
	}	
	
	for(int i = 0; i < numSegments; i++)
	{
		segments[i].Think(elapsedTime);
		
	}
	if(grabbingObject)
	{
		
		//printf("grabPoint = (%f,%f)\n", grabPoint->x, grabPoint->y);
		segments[numSegments - 1].endPoint = *grabPoint + grabObject->position;
	}
	else if(pauseUntilShrink)
	{
		if(GetLength() > BEARD_SHRINK_LENGTH)
			pastMinLength = true;
		if(GetLength() < BEARD_SHRINK_LENGTH && pastMinLength)
		{
			pastMinLength = false;
			pauseUntilShrink = false;
			checkCollision = false;
		}
	}
	return anchorForce;
}
void Rope::DrawQuad(Vector2D p1, Vector2D p2, float width1, float width2)
{
	Vector2D dir = p2 - p1;
	dir = !dir;
	dir = Vector2D(-dir.y, dir.x);
	
	Vector2D a = p1 - width1 * dir;
	Vector2D b = p1 + width1 * dir;
	Vector2D c = p2 - width2 * dir;
	Vector2D d = p2 + width2 * dir;
	
	glVertex2f(a.x, a.y);
	glVertex2f(c.x, c.y);
	glVertex2f(d.x, d.y);
	glVertex2f(b.x, b.y);
}
void Rope::Draw()
{
	float topWidth = 0.5;
	float bottomWidth = 0.1;
	float widthDelta = (topWidth - bottomWidth)/(numSegments);
	
	glColor3f(0.3176,0.0980,0.0706);
	glDisable(GL_TEXTURE_2D);
	glPushMatrix();
	glLoadIdentity();
	glBegin(GL_QUADS);

	DrawQuad(*anchor, segments[0].endPoint, topWidth, topWidth - widthDelta);
	topWidth -= widthDelta;
	for(int i = 1; i < numSegments; i++)
	{
		DrawQuad(segments[i-1].endPoint, segments[i].endPoint, topWidth, topWidth - widthDelta);
		topWidth -= widthDelta;
	}
	glEnd();
	glPopMatrix();
}
